/*
 *  cCalib.h
 *  CalibrationFromVideo
 *
 *  Created by Kris Kitani on 10/19/11.
 *  Copyright 2011 __MyCompanyName__. All rights reserved.
 *
 */

#include <opencv2/opencv.hpp>
#include <iostream>
#include <fstream>
using namespace std;
using namespace cv;

class cCalib{
public:

	bool inited;
	
	cCalib();
	~cCalib();
	
	int analyzeImage(Mat &img);
	int v;
	vector<Mat> im;
	int NC;
	int NR;
	int N;
	int NUM_IMG;
	int SQR_SIZE;
	
	
	vector<Point3f> boardPts;
	vector< vector<Point2f> > imgPts;
	vector<vector<Point3f> > objPts;
	
	Size imSize;


	void calibrate();

	void loadfromfile();

	Mat K,D;\
	
	Mat undistortImg(Mat img);
	
	
};

cCalib::cCalib(){
	NC = 9; 
	NR = 9;
	N = NC*NR;
	//NUM_IMG = 30;
	SQR_SIZE = 1.0;
	v=0;

	
	for(int i = 0; i < NC; i++ )
		for(int j = 0; j < NR; j++) 
			boardPts.push_back(Point3f(i*SQR_SIZE,j*SQR_SIZE,0.0f));

	inited = false;
};

cCalib::~cCalib(){};

int cCalib::analyzeImage(Mat &img){

	if(!inited) loadfromfile();
	
	Mat gry;
	vector<Point2f> corners;
	int flags = 0;
//		CV_CALIB_CB_ADAPTIVE_THRESH+
//		CV_CALIB_CB_FILTER_QUADS+
//		CALIB_CB_FAST_CHECK+
//		CV_CALIB_CB_NORMALIZE_IMAGE;
	
	cvtColor(img,gry,CV_BGR2GRAY);
	bool good = findChessboardCorners(gry,Size(NC,NR),corners,flags);
	
	if(good){
		cornerSubPix(gry, corners, Size(11, 11), Size(-1, -1),
					 TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
		drawChessboardCorners(img,Size(NC,NR),Mat(corners),good);
		
		imshow("corners",img);
		cout << "All detected points correct? Press 'y' or 'n'" << endl;
		char c = waitKey(0);
	
		if(c=='y'){
			imgPts.push_back(corners);
			objPts.push_back(boardPts);
			imSize = gry.size();
			v++;
		}
	}
	
	return v;
}


void cCalib::loadfromfile()
{
	FileStorage fs;
	fs.open("calib_model.xml",FileStorage::READ);
	fs["K"] >> K;
	fs["D"] >> D;
	fs.release();
	inited = true;
	return;
}

void cCalib::calibrate(){
	
	cout << "Computing camera parameters..." << endl;
	
	vector<Mat> rvecs;
    vector<Mat> tvecs;
	
	calibrateCamera(objPts,imgPts,imSize,K,D,rvecs,tvecs);
	cout << "K=" << K << endl;
	cout << "D=" << D << endl;
	
	FileStorage fs;
	
	fs.open("camera-parameters-184.xml",FileStorage::WRITE);
	fs << "K" << K;
	fs << "D" << D;

	fs.open("camera-parameters-184.xml",FileStorage::READ);
	fs["K"] >> K;
	fs["D"] >> D;

	inited = true;
	
}

Mat cCalib::undistortImg(Mat img){
	if(!inited) loadfromfile();
	Mat res;
	cv::undistort(img,res,K,D);
	return res;
}
